//
// Created by jiangbin on 4/22/21.
//

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>
#include "../include/back_wheel.h"
using namespace std;

void back_wheel_w(int s)
{
    int i = 0;
    char cmd = 'w';
    int speed = s;
    FILE *motor_fp[2] = { NULL };
    const char motor_ctl_dir[][150] = {
            {"/sys/class/pwm/pwmchip5/pwm0/duty_cycle"},
            {"/sys/class/pwm/pwmchip8/pwm0/duty_cycle"},
    };

    for (i = 0; i < 2; i++) {
        motor_fp[i] = fopen(motor_ctl_dir[i], "r+");
        if (motor_fp[i] == NULL)
            break;
    }

    if (i == 2) {
        printf("Motor Init\n");
    } else {
        while (i >= 0) {
            fclose(motor_fp[i]);
            i--;
        }
        printf("Motor Error\n");
        return ;
    }

    for(i = 0; i < 2; i++)
        fseek(motor_fp[i], 0L, SEEK_SET);


    switch (cmd) {
        case 'w':
            system("echo 1 > /sys/class/gpio/gpio112/value");
            system("echo 1 > /sys/class/gpio/gpio120/value");
            break;
        case 's':
            system("echo 0 > /sys/class/gpio/gpio112/value");
            system("echo 0 > /sys/class/gpio/gpio120/value");
            break;
        default :
            printf("command format error \n");
            break;
    }

    fprintf(motor_fp[0], "%d", speed);
    fprintf(motor_fp[1], "%d", speed);
    fflush(motor_fp[0]);
    fflush(motor_fp[1]);

    for (i = 0; i < 2; i++) {
        if (motor_fp[i] != NULL) {
            fclose(motor_fp[i]);
            motor_fp[i] = NULL;
        }
    }
}

void back_wheel_s(int s)
{
    int i = 0;
    char cmd = 's';
    int speed = s;
    FILE *motor_fp[2] = { NULL };
    const char motor_ctl_dir[][150] = {
            {"/sys/class/pwm/pwmchip5/pwm0/duty_cycle"},
            {"/sys/class/pwm/pwmchip8/pwm0/duty_cycle"},
    };

    for (i = 0; i < 2; i++) {
        motor_fp[i] = fopen(motor_ctl_dir[i], "r+");
        if (motor_fp[i] == NULL)
            break;
    }

    if (i == 2) {
        printf("Motor Init\n");
    } else {
        while (i >= 0) {
            fclose(motor_fp[i]);
            i--;
        }
        printf("Motor Error\n");
        return ;
    }

    for(i = 0; i < 2; i++)
        fseek(motor_fp[i], 0L, SEEK_SET);


    switch (cmd) {
        case 'w':
            system("echo 1 > /sys/class/gpio/gpio112/value");
            system("echo 1 > /sys/class/gpio/gpio120/value");
            break;
        case 's':
            system("echo 0 > /sys/class/gpio/gpio112/value");
            system("echo 0 > /sys/class/gpio/gpio120/value");
            break;
        default :
            printf("command format error \n");
            break;
    }

    fprintf(motor_fp[0], "%d", speed);
    fprintf(motor_fp[1], "%d", speed);
    fflush(motor_fp[0]);
    fflush(motor_fp[1]);

    for (i = 0; i < 2; i++) {
        if (motor_fp[i] != NULL) {
            fclose(motor_fp[i]);
            motor_fp[i] = NULL;
        }
    }
}



void w_distance(int mm){
    back_wheel_w(7000);
    usleep(mm*50000);
    back_wheel_w(0);
}

void s_distance(int mm){
    back_wheel_s(7000);
    usleep(mm*50000);
    back_wheel_s(0);
}

